//     This program is free software: you can redistribute it and/or modify
//     it under the terms of the GNU General Public License as published by
//     the Free Software Foundation, either version 3 of the License, or
//     (at your option) any later version.
// 
//     This program is distributed in the hope that it will be useful,
//     but WITHOUT ANY WARRANTY; without even the implied warranty of
//     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//     GNU General Public License for more details.
// 
//     You should have received a copy of the GNU General Public License
//     along with this program.  If not, see <http://www.gnu.org/licenses/>.

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <termios.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
       
#include <string.h>

int tty_open(char name[128], speed_t bspeed)
{
	struct termios tios;
	int fd, res;

    fd = open(name, O_RDWR | O_NONBLOCK);

    if (fd < 0)
    {
        fprintf(stderr, "Unable to open port %s\n\r", name);
        return (fd);
    }

    tios.c_cflag = CS8 | CREAD | CLOCAL;
    tios.c_iflag = 0;
    tios.c_oflag = 0;
    tios.c_lflag = 0;

    tios.c_cc[VTIME] = 0;//vtime;                                  // Port timeout: 1/10ths of a second (vtime = 1)
    tios.c_cc[VMIN] = 0;//vmin;                                    // Minimum number of chars to read (vmin = 0)

    cfsetispeed(&tios, bspeed);
    cfsetospeed(&tios, bspeed);

    res = tcsetattr(fd, TCSANOW, &tios);
    if (res)
    {
        fprintf(stderr, "termios: tcsetattr() failed\n\r");
        return (-1);
    }

    return (fd);
}

int main(int argc, char ** argv)
{
	char b[4];
	int i,tmp[5];
    int fd, res;

    if(argc==4) 
	{
		for (i=1; i<argc; i++) tmp[i-1] = atoi(argv[i]);
	} else 
	{
		printf("usage: servo_ser servo(0-7) acceleration(0-127) speed(0-127)\n");
		return -1;
	}

    fd = tty_open("/dev/ttyS1", B9600);
    
	if (fd > 0)
	{
		b[0] = 0x80;
		b[1] = 0x01;
		b[2] = 0x01;
		b[3] = tmp[0];
		b[4] = tmp[1];
        
        res = write(fd, b, 5);
        if(res!=5) printf("res = %d", res);
        
		b[2] = 0x02;
		b[3] = tmp[0];
		b[4] = tmp[2];
        
        res = write(fd, b, 5);
        if(res!=5) printf("res = %d", res);

        close(fd);
        return 0;
	} else
    {
        printf("unable to open file\n");
        return -1;
    }
}

